//
// Created by vivi on 18/03/2018.
//

#pragma once
#include <core/precompiled/precompiled.h>
#include <Eigen/Eigen>
#include "../collider/sdfcollider.h"
#include <ostream>

namespace HairEngine {
	/*
	 * Different from the collider which only handles the distance and gradient query. A collision object provide an
	 * upper level abstraction for handling the transform and have the ability to write the .vply geometry to file.
	 */
	class CollisionObject {
	Public:

		/*
		 * Initaliza a collision object with two transform, the previousTransform could be used to caculate the velocity
		 * of a rigid collision object at the initial state.
		 */
		explicit CollisionObject(const Eigen::Affine3f & transform,
		                         const Eigen::Affine3f & previousTransform,
		                         const float t) {
			updateRigidTransform(transform, t);
			previousLocalToWorldTransform = previousTransform;
		}

		/*
		 * An convenient initalizer which assume that the collsion object is static at the initial state
		 */
		explicit CollisionObject(const Eigen::Affine3f & transform):
				CollisionObject(transform, transform, 1.0f) {}

		/*
		 * We provide a normal solution for transforming the modelDistance and its gradient direction to the world
		 * coordinate. The input worldPos indicates the query world position for input, the outGradientPtr specifies
		 * the gradient direction (we don't ensure the magnitude is right and it has been normalized)
		 */
		virtual float distance(const Eigen::Vector3f & worldPos, Eigen::Vector3f *outGradientPtr = nullptr) const {
			Eigen::Vector3f gradient;

			float distanceInModelSpace = modelDistance(worldToLocalTransform * worldPos, &gradient);

			if (distanceInModelSpace == SDFCollider::SDF_INFINITY)
				return distanceInModelSpace;
			
			float sign = (distanceInModelSpace < 0.0f ? -1.0f : 1.0f);

			//If the gradient at that space is zero, we specify gradient to the x coordinate
			if (gradient == Eigen::Vector3f::Zero())
				gradient = {1.0f, 0.0f, 0.0f};

			//Rescale the length of gradient to the distance in model space and transform it to the world space
			gradient *= std::abs(distanceInModelSpace) / gradient.norm();
			gradient = localToWorldLinear * gradient;

			if (outGradientPtr)
				*outGradientPtr = gradient;

			//We ensure that the transform will not swap handness, thus the model will not flip the outside to the inside,
			//and inside to the outside. So the sign will not be inverse.
			return sign * (gradient.norm());
		}

		const Eigen::Affine3f &getLocalToWorldTransform() const {
			return localToWorldTransform;
		}

		const Eigen::Affine3f &getWorldToLocalTransform() const {
			return worldToLocalTransform;
		}

		/*
		 * Get the velocity for the collision object for a given worldPos.
		 * It won't check whether the worldPos is in the bounding box of not.
		 */
		Eigen::Vector3f velocity(const Eigen::Vector3f & worldPos) const {
			Eigen::Vector3f modelPos = worldToLocalTransform * worldPos;
			Eigen::Vector3f previousWorldPos = previousLocalToWorldTransform * modelPos;
			return (1.0f / deltaTime) * (worldPos - previousWorldPos);
		}

		/*
		 * The axis-aligned bounding box of the collision object, derived class should implement it
		 */
		virtual Eigen::AlignedBox3f boundingBox() const = 0;

		Eigen::Vector3f worldDiagnoal() const {
			Eigen::AlignedBox3f bb = boundingBox();
			return localToWorldTransform * (bb.max() - bb.min());
		}

		/*
		 * Return the distance for a specific query position (in model space) and return a SIGNED distance with a optional
		 * gradient (we don't ensure the magnitude is right and it has been normalized), the graident points out a direction
		 * indicating that the modelPos is increasing fast.
		 */
		virtual float modelDistance(const Eigen::Vector3f & modelPos, Eigen::Vector3f *outGradientPtr = nullptr) const = 0;

		/*
		 * Generate a .vply polygon file stream and output it to the ostream. The base class of collision object does nothing
		 * but return the original ostream, the derived clsas should overrive it if it has something collider to ouput.
		 */
		virtual std::ostream & vplyStream(std::ostream & os) const {
			//Base class do nothing, just return
			return os;
		}

		/*
		 * Update the transformation, it takes the second parameter the t from the last transform to current transform,
		 * which is used to caculate the velocity.
		 */
		void updateRigidTransform(const Eigen::Affine3f & transform, const float t) {
			deltaTime = t;

			previousLocalToWorldTransform = localToWorldTransform;

			localToWorldTransform = transform;
			worldToLocalTransform = localToWorldTransform.inverse();

			localToWorldLinear = localToWorldTransform.linear();
			localToWorldTranslation = localToWorldTransform.translation();
			worldToLocalLinear = worldToLocalTransform.linear();
			worldToLocalTranslation = worldToLocalTransform.translation();
		}
	Protected:
		/*
		 * Two transform to transform the world and local coordinate. We use worldToLocalTransform to transform a point
		 * in the world space to the local space for distance querying. And we use the localToWorldTransform to transform
		 * it back and handling the speed and velocities.
		 */
		Eigen::Affine3f localToWorldTransform, worldToLocalTransform;
		Eigen::Matrix3f localToWorldLinear, worldToLocalLinear;
		Eigen::Vector3f localToWorldTranslation, worldToLocalTranslation;

		//Use for caculating the velocity
		Eigen::Affine3f previousLocalToWorldTransform;

		//The time interval from the last transform to current transform
		float deltaTime;
	};

	/*
	 * Write the collision object of .vply format to a
	 */
	inline std::ostream & operator<<(std::ostream & os, const CollisionObject & collisionObject) {
		collisionObject.vplyStream(os);
		return os;
	}
}